
#include <stdint.h>

#include "board.h"
#include "hpm_gpio_drv.h"
#include "hpm_gpiom_regs.h"
#include "hpm_gpiom_drv.h"

#include "jtag.h"

#define JTAG_TCK  IOC_PAD_PD19
#define JTAG_TMS  IOC_PAD_PD20
#define JTAG_TDI  IOC_PAD_PD23
#define JTAG_TDO  IOC_PAD_PD24
#define JTAG_TRST IOC_PAD_PD25

static void gpiom_configure_pin_control_setting(uint16_t gpio_index)
{
    // printf("using gpiom configures pin control module\n");
    gpiom_set_pin_controller(HPM_GPIOM, GPIO_GET_PORT_INDEX(gpio_index), GPIO_GET_PIN_INDEX(gpio_index), gpiom_core0_fast);
    gpiom_enable_pin_visibility(HPM_GPIOM, GPIO_GET_PORT_INDEX(gpio_index), GPIO_GET_PIN_INDEX(gpio_index), gpiom_core0_fast);
    gpiom_lock_pin(HPM_GPIOM, GPIO_GET_PORT_INDEX(gpio_index), GPIO_GET_PIN_INDEX(gpio_index));
}

void jtag_pin_init(void)
{
    HPM_IOC->PAD[JTAG_TCK].FUNC_CTL = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(0);
    HPM_IOC->PAD[JTAG_TMS].FUNC_CTL = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(0);
    HPM_IOC->PAD[JTAG_TDI].FUNC_CTL = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(0);
    HPM_IOC->PAD[JTAG_TDO].FUNC_CTL = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(0);

    gpiom_configure_pin_control_setting(JTAG_TCK);
    gpio_set_pin_output(HPM_FGPIO, GPIO_GET_PORT_INDEX(JTAG_TCK), GPIO_GET_PIN_INDEX(JTAG_TCK));

    gpiom_configure_pin_control_setting(JTAG_TMS);
    gpio_set_pin_output(HPM_FGPIO, GPIO_GET_PORT_INDEX(JTAG_TMS), GPIO_GET_PIN_INDEX(JTAG_TMS));

    gpiom_configure_pin_control_setting(JTAG_TDI);
    gpio_set_pin_output(HPM_FGPIO, GPIO_GET_PORT_INDEX(JTAG_TDI), GPIO_GET_PIN_INDEX(JTAG_TDI));

    gpiom_configure_pin_control_setting(JTAG_TDO);
    gpio_set_pin_input(HPM_FGPIO, GPIO_GET_PORT_INDEX(JTAG_TDO), GPIO_GET_PIN_INDEX(JTAG_TDO));

    HPM_IOC->PAD[JTAG_TCK].PAD_CTL = IOC_PAD_PAD_CTL_PE_SET(1) | IOC_PAD_PAD_CTL_PS_SET(1);
    HPM_IOC->PAD[JTAG_TMS].PAD_CTL = IOC_PAD_PAD_CTL_PE_SET(1) | IOC_PAD_PAD_CTL_PS_SET(1);
    HPM_IOC->PAD[JTAG_TDI].PAD_CTL = IOC_PAD_PAD_CTL_PE_SET(1) | IOC_PAD_PAD_CTL_PS_SET(1);
    HPM_IOC->PAD[JTAG_TDO].PAD_CTL = IOC_PAD_PAD_CTL_PE_SET(1) | IOC_PAD_PAD_CTL_PS_SET(1);

    // HPM_IOC->PAD[JTAG_TRST].FUNC_CTL = IOC_PAD_FUNC_CTL_ALT_SELECT_SET(0);
    // gpiom_configure_pin_control_setting(JTAG_TRST);
    // gpio_set_pin_input(HPM_FGPIO, GPIO_GET_PORT_INDEX(JTAG_TRST), GPIO_GET_PIN_INDEX(JTAG_TRST));
    // HPM_IOC->PAD[JTAG_TRST].PAD_CTL = IOC_PAD_PAD_CTL_PE_SET(1) | IOC_PAD_PAD_CTL_PS_SET(1);
}
//
#define jtag_pin_tck_low()  (HPM_FGPIO->DO[GPIO_GET_PORT_INDEX(JTAG_TCK)].CLEAR = 1 << GPIO_GET_PIN_INDEX(JTAG_TCK))
#define jtag_pin_tck_high() (HPM_FGPIO->DO[GPIO_GET_PORT_INDEX(JTAG_TCK)].SET = 1 << GPIO_GET_PIN_INDEX(JTAG_TCK))
#define jtag_pin_tms_low()  (HPM_FGPIO->DO[GPIO_GET_PORT_INDEX(JTAG_TMS)].CLEAR = 1 << GPIO_GET_PIN_INDEX(JTAG_TMS))
#define jtag_pin_tms_high() (HPM_FGPIO->DO[GPIO_GET_PORT_INDEX(JTAG_TMS)].SET = 1 << GPIO_GET_PIN_INDEX(JTAG_TMS))
#define jtag_pin_tdi_low()  (HPM_FGPIO->DO[GPIO_GET_PORT_INDEX(JTAG_TDI)].CLEAR = 1 << GPIO_GET_PIN_INDEX(JTAG_TDI))
#define jtag_pin_tdi_high() (HPM_FGPIO->DO[GPIO_GET_PORT_INDEX(JTAG_TDI)].SET = 1 << GPIO_GET_PIN_INDEX(JTAG_TDI))
#define jtag_pin_tdo(val)   do{val = (HPM_FGPIO->DI[GPIO_GET_PORT_INDEX(JTAG_TDO)].VALUE & (1 << GPIO_GET_PIN_INDEX(JTAG_TDO))) >> GPIO_GET_PIN_INDEX(JTAG_TDO);} while (0)
#define jtag_pin_delay(tick)do{for (volatile int i = 0; i < tick; i++){asm volatile("nop");}} while (0)

//
#define JTAG_TICK (10)
#define jtag_cycle_tck()           \
    do                             \
    {                              \
        jtag_pin_tck_low();        \
        jtag_pin_delay(JTAG_TICK); \
        jtag_pin_tck_high();       \
        jtag_pin_delay(JTAG_TICK); \
    } while (0)

#define jtag_cycle_tdi(tdi)                             \
    do                                                  \
    {                                                   \
        tdi ? jtag_pin_tdi_high() : jtag_pin_tdi_low(); \
        jtag_pin_tck_low();                             \
        jtag_pin_delay(JTAG_TICK);                      \
        jtag_pin_tck_high();                            \
        jtag_pin_delay(JTAG_TICK);                      \
    } while (0)

#define jtag_cycle_tdo(tdo)        \
    do                             \
    {                              \
        jtag_pin_tck_low();        \
        jtag_pin_delay(JTAG_TICK); \
        jtag_pin_tdo(tdo);         \
        jtag_pin_tck_high();       \
        jtag_pin_delay(JTAG_TICK); \
    } while (0)

#define jtag_cycle_tdio(tdi, tdo)                       \
    do                                                  \
    {                                                   \
        tdi ? jtag_pin_tdi_high() : jtag_pin_tdi_low(); \
        jtag_pin_tck_low();                             \
        jtag_pin_delay(JTAG_TICK);                      \
        jtag_pin_tdo(tdo);                              \
        jtag_pin_tck_high();                            \
        jtag_pin_delay(JTAG_TICK);                      \
    } while (0)

//

void jtag_tap_reset(void)
{
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    for (int i = 0; i < 8; i++)
    {
        jtag_cycle_tck();
    }
}

void jtag_tap_idle(int _tick)
{
    for (int i = 0; i < _tick; i++)
    {
        //jtag_pin_tms(0);
        jtag_pin_tms_low();
        jtag_cycle_tck();
    }
}

void jtag_tap_init(void)
{
    jtag_tap_reset();
    jtag_tap_idle(1);
}

void jtag_irlen_tapnum(void)
{
    uint8_t _tdo = 0;
    irlen = 0;
    //jtag_tap_idle();

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Select-DR

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Select-IR

    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Capture-IR

    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Shift-IR

    for (int i = 0; i < 32; i++)
    {
        jtag_cycle_tdi(0);
    }

    for (int i = 0; i < 32; i++)
    {
        if (i == 32 - 1)
        {
            //jtag_pin_tms(1);    // Exit1-IR
            jtag_pin_tms_high();
        }

        jtag_cycle_tdio(1, _tdo);
        if (_tdo == 0)
        {
            irlen++;
        }
    }

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Update-IR
    
    //jtag_pin_tms(0);
    //jtag_cycle_tck(); // Idle
    ////  BYPASS
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Select-DR
    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Capture-DR
    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Shift-DR

    for (int i = 0; i < 32; i++)
    {
        jtag_cycle_tdi(0);
    }

    for (int i = 0; i < 32; i++)
    {
        if (i == 32 - 1)
        {
            //jtag_pin_tms(1);    // Exit1-DR
            jtag_pin_tms_high();
        }

        jtag_cycle_tdio(1, _tdo);
        if (_tdo == 0)
        {
            tapnum++;
        }
    }

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Update-DR
    //// 
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Select-DR

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Select-IR

    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Capture-IR

    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Shift-IR

    for (int i = 0; i < 5; i++)
    {
        if (i == 5 - 1)
        {
            //jtag_pin_tms(1);    // Exit1-IR
            jtag_pin_tms_high();
        }
        #define RISCV_DTM_JTAG_REG_IDCODE 0x01
        jtag_cycle_tdio((RISCV_DTM_JTAG_REG_IDCODE >> i) & 1, _tdo);
    }

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Update-IR

    ////  IDCODE
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Select-DR
    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Capture-DR
    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tck(); // Shift-DR

    for (int i = 0; i < 32; i++)
    {
        if (i == 32 - 1)
        {
            //jtag_pin_tms(1);    // Exit1-DR
            jtag_pin_tms_high();
        }

        jtag_cycle_tdo(_tdo);
        idcode |= ((_tdo & 1) << i);
    }

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tck(); // Update-DR

    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tdi(1); // Run-Test/Idle
}

// 对于 *oldval 调用者一定要做清零
static void jtag_tap_shift(uint32_t *oldval, uint32_t *newval, uint32_t len)
{
    volatile uint32_t _tdo;
    volatile uint32_t _tdi;

    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tdi(1); // Capture-DR/IR
    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tdi(1); // Shift-DR/IR

    for (uint32_t i = 0; i < len; i++)
    {
        _tdi = (newval[i / var_bits(uint32_t)] >> (i % var_bits(uint32_t))) & 1;
        if (i == len - 1)
        {
            //jtag_pin_tms(1);
            jtag_pin_tms_high();
            jtag_cycle_tdio(_tdi, _tdo); // Exit1-DR/IR
        }
        else
        {
            //jtag_pin_tms(0);
            jtag_pin_tms_low();
            jtag_cycle_tdio(_tdi, _tdo); // Shift-DR/IR
        }
        oldval[i / var_bits(uint32_t)] |= ((_tdo & 1) << (i % var_bits(uint32_t)));
    }

    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tdi(1); // Update-DR/IR
    //jtag_pin_tms(0);
    jtag_pin_tms_low();
    jtag_cycle_tdi(1); // Run-Test/Idle

    //asm volatile("ebreak"); 
    return;
}

void jtag_tap_shift_ir(uint32_t *oldval, uint32_t *newval, int len)
{
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tdi(1); // Select-DR-Scan
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tdi(1); // Select-IR-Scan
    jtag_tap_shift(oldval, newval, len);
}

void jtag_tap_shift_dr(uint32_t *oldval, uint32_t *newval, int len)
{
    //jtag_pin_tms(1);
    jtag_pin_tms_high();
    jtag_cycle_tdi(1); // Select-DR-Scan
    jtag_tap_shift(oldval, newval, len);
}

uint32_t irlen = 5;
uint32_t tapnum = 0;
uint32_t idcode;
